TP2 - Filtros de Kalman

Elementos de Matemáticas Aplicadas para Aplicaciones Tecnológicas

Autor/a
Afiliación

Matías Roqueta

Instituto Balseiro

Descripción del problema

Se implementará un filtro de Kalman para fusionar los datos de sensores presentes en un vehículo para estimar la posición y orientación del mismo en dos dimensiones. Un diagrama del mismo se presenta en la Figura 1.

Figura 1: Esquema del sistema.

En la figura se representan los ejes \(x\) e \(y\), llamados el sistema de referencia inercial. Se repesenta además el eje \(x_b\), el cual junto con un eje \(y_b\) ortogonal al mismo forman el sistema de referencia del móvil, llamado body. El filtro de Kalman estimará la posición del móvil en el sistema de referencia intercial, \(x_{est},\, y_{est}\) así como la orientación del mismo, dada por el ángulo \(\theta\).

Los instrumentos disponibles para la estimación de la posición son los siguientes.

  • Unidad de Medición Inercial (IMU): Consiste en un giróscopo y un acelerómetro los cuales miden a una tasa \(f_s\).
    • Acelerómetro: Mide la aceleración lineal del móvil en el sistema body, \(u^{x_b} = \ddot{x}_b\) y \(u^{y_b} = \ddot{y}_b\).
    • Giróscopo: Mide la velocidad angular del móvil, \(u^{\theta} = \dot{\theta}\).
  • GPS: Mide la posición del móvil en el sistema de referencia inercial a una tasa \(f_s'\).
  • Magnetómetro: Por medio de mediciones del campo magnético terrestre calcula la orientación del móvil, también a tasa \(f_s'\).

Error de los instrumentos

Para cada dimensión de medición se considera su correspondiente error

Código
using LinearAlgebra
using Distributions
using Plots
using PlotThemes

theme(:wong2)

## Parametros de error instrumentos

Δt = 0.1

fs = 10
fs2 = 1
PSD_a = 80*10*1e-6
PSD_g = 0.03*π/180
PSD_m = 100e-6*1e-4
B_T = 25358e-9

err_IMU_x = PSD_a*sqrt(fs)
err_IMU_θ = PSD_g*sqrt(fs)

err_GPS = 2.5
err_MAG = PSD_m*sqrt(fs2)/B_T

Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]

R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

Filtro de Kalman

El filtro de Kalman se hará sobre el estimador del estado de posición, velocidad, y orientación del móvil en el sistema de referencia inercial

\[ \mathbf{\hat{x}}_k = \begin{bmatrix}\hat{x}_k \\ \hat{\dot{x}}_k \\ \hat{y}_k \\ \hat{\dot{y}}_k \\ \hat{\theta}_k \end{bmatrix} \]

Medición del IMU

A cada instante de muestreo del IMU se obtiene un vector de mediciones \(\mathbf{\overline{u}}_k\), el cual se define como su valor real \(\mathbf{u}_k\) más su error \(\mathbf{w'}_k\)

\[ \mathbf{\overline{u}}_k = \mathbf{u}_k + \mathbf{w}_k' = \begin{bmatrix}u_k^{x_b} \\ u_k^{y_b} \\ u_k^{\theta} \end{bmatrix} + \mathbf{w}_k' \]

Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por

\[ Q = \begin{bmatrix} (\sigma^{x_b})^2 & 0 & 0\\ 0 & (\sigma^{x_b})^2 & 0\\ 0 & 0 & (\sigma^\theta)^2\\ \end{bmatrix} \]

Medición del GPS/Magnetómetro

Cuando \(k\) corresponde a un instante de muestreo del GPS, se obtiene un vector de mediciones \(\mathbf{\overline{z}}_k\), el cual se define como su valor real \(\mathbf{z}_k\) más su error \(\mathbf{v}_k\)

\[ \mathbf{\overline{z}}_k = \mathbf{z}_k + \mathbf{v}_k = \begin{bmatrix}z_k^{x} \\ z_k^{y} \\ z_k^{\theta} \end{bmatrix} + \mathbf{v}_k \]

En ausencia de aceleración, la evolución del estado en un tiempo \(\Delta t\) seguiría un movimiento rectilíneo uniforme, dado por

\[ \mathbf{x}_k = \begin{bmatrix}x_k \\ \dot{x}_k \\ y_k \\ \dot{y}_k \\ \theta_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & \Delta t & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}x_{k-1} \\ \dot{x}_{k-1} \\ y_{k-1} \\ \dot{y}_{k-1} \\ \theta_{k-1} \end{bmatrix} \]

Modelo de actualización de estado

La actualización del estado se hace en función de las mediciones de la IMU, que informan la aceleración del móvil en el sistema de referencia body, estas se modelan como el vector de medición \(\mathbf u\) más el vector de error \(\mathbf w'\)

Estas aceleraciones se transladan al sistema de referencia inercial y se utilizan para realizar la actualización por modelo del filtro de Kalman, dada por medio de la ecuación

\[ \mathbf{\hat{x}}_k = A\mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} \]

En donde la matriz \(B_{k-1}\) que obtiene la actualización del estado en el sistema inercial \(\mathbf{x}\) en función de las mediciones de aceleración en el sistema body es dada por

\[ B_{k-1} = \begin{bmatrix} \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & - \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & 0 \\ \Delta t \cos(\theta_{k-1}) & - \Delta t \sin(\theta_{k-1}) & 0 \\ \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & 0 \\ \Delta t \sin(\theta_{k-1}) & \Delta t \cos(\theta_{k-1}) & 0 \\ 0 & 0 & \Delta t \end{bmatrix} \]

Y el ruido es similarmente transformado, obteniendo \(\mathbf{w}_{k-1} = B \mathbf{w}_{k-1}'\). La matriz de covarianza que resulta de la transformación es dada por

\[ Q_k = B Q B^T \]

Modelo de actualización por medición

Código
A = [1 Δt 0 0 0;
     0 1 0 0 0;
     0 0 1 Δt 0;
     0 0 0 1 0;
     0 0 0 0 1]

function B_matrix(θ)
    return [Δt^2*cos(θ)/2 -Δt^2*sin(θ)/2 0;
    Δt*cos(θ) -Δt*sin(θ) 0;
    Δt^2*sin(θ)/2 Δt^2*cos(θ)/2 0;
    Δt*sin(θ) Δt*cos(θ) 0;
    0 0 Δt]
end

H = [1 0 0 0 0;
     0 0 1 0 0;
     0 0 0 0 1]

Implementación del algoritmo de estimación

Inicialización

Código
function kalman_initialize(t, z)
    N = length(t)
    x = zeros(5, N)
    x[1:2:5,1] = z[:,1]
    P = zeros(5, 5, N)
    P[:,:,1] = diagm([err_GPS^2,(err_GPS*fs2)^2,err_GPS^2,(err_GPS*fs2)^2,2pi])
    K = zeros(5, 3, N)
    return x, P, K
end
kalman_initialize (generic function with 1 method)

Predicción

En cada instante de muestreo del IMU se realiza la etapa de predicción

  • Estimación a priori: \(\mathbf{\hat{x}}_k^{-} = \mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{\overline{u}}_{k-1}\)
  • Covarianza a priori: \(P_k^{-} = A P_{k-1}A^T + Q_k\)
  • Ganancia de Kalman: \(K_k = P^{-}_k H^T (H P_k^{-}H^T + R_k)^{-1}\)
Código
function kalman_predict(x, u, P)
    B = B_matrix(x[5])
    x_est = A*x + B*u
    P_est = A*P*A' + B*Q*B'
    K = P_est*H'*inv(H*P_est*H'+R)
    return x_est, P_est, K
end
kalman_predict (generic function with 1 method)

Corrección

En cada instante de muestreo del GPS/magnetómetro, se aplica una corrección por medición al estimador.

  • Estimador a posteriori: \(\mathbf{\hat{x}}_k^{+} = \mathbf{\hat{x}}_k^{-} + K_k (\mathbf{\overline{z}}_k-H\mathbf{\hat{x}}_k^{-})\)
  • Covarianza a posteriori: \(P_k^{+} = (I-K_k H)P_k^{-}\)
Código
function kalman_correct(x_prior, z, P_prior, K)
    x_est = x_prior + K*(z-H*x_prior)
    P_est = (I-K*H)*P_prior
    return x_est, P_est
end
kalman_correct (generic function with 1 method)
Código
function kalman(t, tasa_GPS, u, z)
    x, P, K = kalman_initialize(t, z)
    for k in 2:length(t)
        x_est, P_est, K_k = kalman_predict(x[:,k-1], u[:,k], P[:,:,k-1])
        if k%tasa_GPS == 0
            x_est, P_est = kalman_correct(x_est, z[:,k], P_est, K_k)
        end
        x[:,k] = x_est
        P[:,:,k] = P_est
        K[:,:,k] = K_k
    end
    return x, P, K
end
kalman (generic function with 1 method)

Estimación de diferentes trayectorias

En todas las trayectorias, los ruidos de los instrumentos se modelan como variables aleatorias gaussianas de media \(0\), con muestras independientes e idénticamente distribuidas.

\[ \mathbf{w}'_k \sim \mathcal{N}\left[\mathbf{0}, Q\right] \qquad \mathbf{v}_k \sim \mathcal{N}\left[\mathbf{0}, R\right] \]

Código
T = 500
t = 0:0.1:T

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

Móvil estático

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \]

\[ \mathbf{\overline{z}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{v}_k \]

Código
function estatico_IMU()
    return [0,0,0]
end
function estatico_GPS()
    return [0,0,0]
end

u_true = hcat([estatico_IMU() for ti in t]...)
z_true = hcat([estatico_GPS() for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
Código
function trayectoria_xy(x, z_true)
    p1 = plot(x[1,:], x[3,:], label="Estimación", xlabel="x", ylabel="y", aspect_ratio=:equal, title="Trayectoria XY")
    plot!(p1, z_true[1,:], z_true[2,:], label="Trayectoria")
    return p1
end

trayectoria_xy(x, z_true)
Código
function trayectoria_t(t, x, z_true)
    p1 = plot(t, x[1,:], ylabel="x [m]", title="Trayectoria en función del tiempo")
    plot!(p1, t, z_true[1,:])
    p2 = plot(t, x[3,:], ylabel="y [m]")
    plot!(p2, t, z_true[2,:])
    p3 = plot(t, x[5,:] .% 2pi, ylabel="θ", xlabel="t [s]")
    plot!(p3, t, z_true[3,:] .% 2pi)
    return plot(p1,p2,p3, layout=(3,1), legend=false)
end

trayectoria_t(t, x, z_true)

Movimiento rectilíneo uniformemente acelerado

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} a \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \]

\[ \mathbf{\overline{z}}_k = \begin{bmatrix} \frac 1 2 a t_k^2 \cos(\theta) \\ \frac 1 2 a t_k^2 \sin(\theta) \\ \theta \end{bmatrix} + \mathbf{v}_k \]

Código
function mrua_IMU(a)
    return [a,0,0]
end
function mrua_GPS(a, t, θ)
    return [1/2 * a * t^2 * cos(θ), 1/2 * a* t^2 * sin(θ), θ]
end

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
Código
trayectoria_xy(x, z_true)
Código
trayectoria_t(t, x, z_true)

Movimiento circular uniforme

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ \omega^2 r \\ \omega \end{bmatrix} + \mathbf{w}_k' \]

\[ \mathbf{\overline{z}}_k = \begin{bmatrix} r \cos(\omega t_k) \\ r \sin(\omega t_k) \\ \omega t_k + \frac \pi 2 \end{bmatrix} + \mathbf{v}_k \]

Código
function circular_IMU(r, w, t)
    return [0,w^2*r,w] 
end

function circular_GPS(r, w, t)
    return [r*cos(w*t),r*sin(w*t),w*t+π/2] 
end

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
Código
trayectoria_xy(x, z_true)
Código
trayectoria_t(t, x, z_true)

Análisis de desempeño

Error de estimación

Se procede a cuantificar el error de la estimación, el cual se define como la norma cuadrática de la distancia entre la estimación y el valor real

\[ \epsilon_k = \left\lVert\begin{bmatrix}\hat{x}_k\\\hat{y}_k\end{bmatrix}- \begin{bmatrix}z^x_k\\ z^y_k\end{bmatrix}\right\rVert^2 \]

Código
function error_cuadratico(x, z_true)
    xs = [[x_i[1] x_i[3]] for x_i in eachcol(x)]
    zs = [[z_i[1] z_i[2]] for z_i in eachcol(z_true)]
    return [norm(x_i-z_i)^2 for (x_i, z_i) in zip(xs, zs)]
end
error_cuadratico (generic function with 1 method)

Esto se compara con la incerteza del estimador, la cual se cuantifica con la traza de la matriz de covarianza \(P\)

Código
function varianza_estimador(P)
    return [tr(P[:,:,i]) for i in 1:size(P,3)]
end
varianza_estimador (generic function with 1 method)
Código
function plot_error(t, x, z_true, P)
    v = varianza_estimador(P)
    e = error_cuadratico(x, z_true)
    p1 = plot(t, v, ylim=[0, maximum(v)*1.1], ylabel="Incerteza")
    p2 = plot(t, e, ylim=[0, maximum(e)*1.1], ylabel="Error cuadrático", xlabel="t")
    return plot(p1, p2, layout=(2,1))
end

plot_error(t, x, z_true, P)

Ganancia de Kalman

Código
function kalman_gain(K)
    return [norm(K[:,:,i]) for i in 1:size(K,3)]
end
kalman_gain (generic function with 1 method)
Código
function plot_gain(t, P, K)
    v = varianza_estimador(P)
    k = kalman_gain(K)
    p1 = plot(t, v, ylim=[0, maximum(v)*1.1], ylabel="Incerteza")
    p2 = plot(t, k, ylim=[0, maximum(k)*1.1], ylabel="Ganancia Kalman", xlabel="t")
    return plot(p1, p2, layout=(2,1))
end
plot_gain(t, P, K)
Código
k1 = 4000
k2 = 4050
plot_gain(t[k1:k2], P[:,:,k1:k2], K[:,:,k1:k2])

Efecto de la incerteza de los instrumentos

Incrementa x5 error de GPS

Código
Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]
R = [(5*err_GPS)^2 0 0; 0 (5*err_GPS)^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
([6.399781199194112 6.3997990738809545 … 45.341589260594674 45.36055364889119; 0.0 0.00035749373686201424 … 0.18985890889783308 0.18942885703251805; … ; 0.0 0.00021385783699033828 … 0.1557259904736974 0.15562015104786917; 0.7848488405492863 0.7849244225793554 … 0.7857678701547156 0.7854209220538022], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.31250000016 0.6250000032 … -3.269996546517461e-26 0.0; 0.6250000032 6.250000064 … 4.027810561325809e-24 0.0; … ; 1.5455836360409911e-25 -4.5202712722881835e-24 … 6.250000064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 6.5000000016 1.2500000128000002 … 4.860834460651788e-25 0.0; 1.2500000128000002 6.250000128 … 3.802909870361181e-24 0.0; … ; -4.120565610610859e-25 -4.7063484492424615e-24 … 6.250000128 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 1.789434126214376 0.010048778940915963 … 1.107835787193436e-21 0.0; 0.010048778940915964 0.00011380649969204478 … 2.3689206546853487e-23 0.0; … ; 1.9286714934979128e-21 7.105349407547785e-24 … 0.00011380649969204478 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 1.7711384781044608 0.009946127965829606 … 1.1028462603811658e-21 0.0; 0.009946127965829608 0.00011323011782660337 … 2.266065265163026e-23 0.0; … ; 1.8874621364388075e-21 5.740016248312654e-24 … 0.00011323011782660337 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 1.773128836158805 0.009957454177612266 … 1.1049421360554004e-21 0.0; 0.009957454177612268 0.00011329411782660338 … 2.580537468853133e-23 0.0; … ; 1.8877920421337128e-21 7.9209010990109e-24 … 0.00011329411782660338 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.038831218762960626 -5.659833239126597e-29 0.0; 0.0038446751446328943 -6.915408885968804e-28 0.0; … ; 9.505566154937311e-28 0.0038446751446328943 0.0; 0.0 0.0 0.9999999752491497;;; 0.03993855607702672 9.813851658963173e-29 0.0; 0.007680491630032021 2.580844671204317e-27 0.0; … ; -2.530169321216744e-27 0.007680491630032021 0.0; 0.0 0.0 0.9999999752491497;;; … ;;; 0.011322706488465957 -5.024560499576099e-22 0.0; 6.358399722496316e-5 7.001907088186049e-24 0.0; … ; 1.2075519151180111e-23 6.358399722496316e-5 0.0; 0.0 0.0 0.6968640780830011;;; 0.011335286259868549 -5.010596696256948e-22 0.0; 6.365521898130948e-5 7.016279173975292e-24 0.0; … ; 1.2079757673208367e-23 6.365521898130948e-5 0.0; 0.0 0.0 0.7122418117142865;;; 0.0112206918646524 -4.885008084320078e-22 0.0; 6.301263777618486e-5 6.984084008668774e-24 0.0; … ; 1.1821788768628917e-23 6.301263777618486e-5 0.0; 0.0 0.0 0.47048808605167836])
Código
trayectoria_xy(x, z_true)
Código
trayectoria_t(t, x, z_true)
Código
u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
([26.399781199194113 26.399770484752086 … 21.137718470315665 21.139276033568887; 0.0 -0.00021428884050346046 … 0.015848432720687408 0.015302832343740299; … ; 0.0 0.00035418007102216406 … 0.23224185555942428 0.23181349171664725; 1.5702470039467347 1.5715792230382397 … 7.8530947036703145 7.854004392630837], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.31250000016 0.6250000032 … 1.6087096783876353e-28 0.0; 0.6250000032 6.250000064 … 7.256472083863048e-27 0.0; … ; -2.5780275752747166e-28 -3.492159999452689e-27 … 6.250000064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 6.5000000016 1.2500000128000002 … 8.119845002821807e-28 0.0; 1.2500000128000002 6.250000128 … 3.938911542442671e-27 0.0; … ; -4.297693240627179e-28 2.5472535823627876e-27 … 6.250000128 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 1.789434126214376 0.010048778940915963 … -1.084727966180855e-21 0.0; 0.010048778940915964 0.00011380649969204478 … -2.2408183550581983e-23 0.0; … ; 4.222681407684344e-21 9.498701955016883e-23 … 0.00011380649969204478 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 1.7711384781044608 0.009946127965829606 … -1.067773155787924e-21 0.0; 0.009946127965829608 0.00011323011782660337 … -2.2285538000289733e-23 0.0; … ; 4.166316871199004e-21 9.446469265752443e-23 … 0.00011323011782660337 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 1.773128836158805 0.009957454177612266 … -1.0700015261885246e-21 0.0; 0.009957454177612268 0.00011329411782660338 … -2.228936724195128e-23 0.0; … ; 4.175763602472809e-21 9.446973240542751e-23 … 0.00011329411782660338 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.038831218762960626 1.0065668248689088e-31 0.0; 0.0038446751446328943 1.2244906599188314e-30 0.0; … ; -1.5856425077993764e-30 0.0038446751446328943 0.0; 0.0 0.0 0.9999999752491497;;; 0.03993855607702672 7.872090306349743e-31 0.0; 0.007680491630032021 5.335970871632301e-30 0.0; … ; -2.6365881045100373e-30 0.007680491630032021 0.0; 0.0 0.0 0.9999999752491497;;; … ;;; 0.011322706488465957 -6.819089261947242e-22 0.0; 6.358399722496316e-5 -6.907035452039849e-24 0.0; … ; 2.66051354608374e-23 6.358399722496316e-5 0.0; 0.0 0.0 0.6968640780830011;;; 0.011335286259868549 -6.832587527973631e-22 0.0; 6.365521898130948e-5 -6.920990849239914e-24 0.0; … ; 2.666442797567362e-23 6.365521898130948e-5 0.0; 0.0 0.0 0.7122418117142865;;; 0.0112206918646524 -6.693576799048734e-22 0.0; 6.301263777618486e-5 -6.814780531329688e-24 0.0; … ; 2.6313887273236363e-23 6.301263777618486e-5 0.0; 0.0 0.0 0.47048808605167836])
Código
trayectoria_xy(x, z_true)
Código
trayectoria_t(t, x, z_true)
Código
plot_error(t, x, z_true, P)
Código
plot_gain(t[k1:k2], P[:,:,k1:k2], K[:,:,k1:k2])

Incrementa x5 error de IMU

Código
Q = [(5*err_IMU_x)^2 0 0; 0 (5*err_IMU_x)^2 0; 0 0 (5*err_IMU_θ)^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
([-4.053193522251121 -4.053107450389068 … 44.57735438453592 44.596830676069956; 0.0 0.0017214372410541373 … 0.19484425526926719 0.19468157541137582; … ; 0.0 -0.0008373128802501247 … 0.23659620376470644 0.23761825355134944; 0.7855433303676169 0.786645576427757 … 0.7857587554823343 0.7868058466499893], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.312500004 0.62500008 … 9.529706604467709e-24 0.0; 0.62500008 6.2500016 … 1.572470425413841e-23 0.0; … ; 5.5265649441002705e-24 2.8199545226197805e-24 … 6.2500016 0.0; 0.0 0.0 … 0.0 6.2831859925687805;;; 6.500000040000001 1.25000032 … 1.7739246973428253e-23 0.0; 1.25000032 6.2500032 … 7.593671972830343e-23 0.0; … ; 2.3009522671901827e-26 -1.2076188861814014e-22 … 6.2500032 0.0; 0.0 0.0 … 0.0 6.283186677957975;;; … ;;; 0.36169288198748656 0.010229584529472202 … -1.8396903206497448e-21 0.0; 0.010229584529472202 0.0005721606635760474 … 6.823817069246597e-23 0.0; … ; 2.3993859677098043e-21 2.6112634200199806e-22 … 0.0005721606635760474 0.0; 0.0 0.0 … 0.0 6.320639916249882e-6;;; 0.3437392039718934 0.009721119932251493 … -1.6289303424767417e-21 0.0; 0.009721119932251493 0.0005577606635759174 … 7.340192039930846e-23 0.0; … ; 2.190209028955388e-21 2.7648033253749534e-22 … 0.0005577606635759174 0.0; 0.0 0.0 … 0.0 1.5213716556903334e-7;;; 0.34568900956497944 0.009776975998609085 … -1.6190272842674518e-21 0.0; 0.009776975998609085 0.0005593606635759175 … 6.176572960357224e-24 0.0; … ; 2.2184080698883454e-21 3.855958732282835e-22 … 0.0005593606635759175 0.0; 0.0 0.0 … 0.0 8.375263600891277e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.5024875623474667 4.044407862005418e-27 0.0; 0.049751250133412535 -2.7043607576388794e-25 0.0; … ; 4.405483851296487e-25 0.049751250133412535 0.0; 0.0 0.0 0.9999999752491522;;; 0.5098039231064976 3.5591829840556046e-26 0.0; 0.09803924047673965 2.970478437021516e-25 0.0; … ; 4.032043718105931e-27 0.09803924047673965 0.0; 0.0 0.0 0.999999975249155;;; … ;;; 0.054705033709726854 -9.921045094305651e-21 0.0; 0.001547195962072148 -2.707088229923479e-22 0.0; … ; 3.4665418277225106e-22 0.001547195962072148 0.0; 0.0 0.0 0.9759866455818303;;; 0.05499827263550295 -9.968180513867565e-21 0.0; 0.0015553791891602389 -2.6961327433362486e-22 0.0; … ; 3.5043344463286204e-22 0.0015553791891602389 0.0; 0.0 0.0 0.9782848225201974;;; 0.05241135673068665 -8.998144244783633e-21 0.0; 0.0014823282274877797 -2.4044713562199325e-22 0.0; … ; 3.2221526381863383e-22 0.0014823282274877797 0.0; 0.0 0.0 0.8433959372095443])
Código
trayectoria_xy(x, z_true)
Código
trayectoria_t(t, x, z_true)
Código
u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)
([15.94680647774888 15.946881152223028 … 20.167144093856205 20.166095811845853; 0.0 0.001493489482977554 … -0.009906401051208671 -0.011059239155777875; … ; 0.0 0.0005751244837655708 … 0.3073835570019056 0.3079425585645642; 1.5709414937650652 1.5733003768866411 … 7.853085588997933 7.855389317227024], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.312500004 0.62500008 … -1.1829558670837016e-27 0.0; 0.62500008 6.2500016 … 1.5743550327490137e-26 0.0; … ; -1.5598932487744637e-27 -2.1465242046224327e-26 … 6.2500016 0.0; 0.0 0.0 … 0.0 6.2831859925687805;;; 6.500000040000001 1.25000032 … -1.224802040086607e-26 0.0; 1.25000032 6.2500032 … -3.2902646932283077e-26 0.0; … ; -1.31818563583822e-26 -3.978384850805885e-26 … 6.2500032 0.0; 0.0 0.0 … 0.0 6.283186677957975;;; … ;;; 0.36169288198748656 0.010229584529472202 … -6.6133369889639375e-21 0.0; 0.010229584529472202 0.0005721606635760474 … -4.308123134440125e-22 0.0; … ; 5.50852070862659e-21 3.353908489802122e-22 … 0.0005721606635760474 0.0; 0.0 0.0 … 0.0 6.320639916249882e-6;;; 0.3437392039718934 0.009721119932251493 … -6.115763448215439e-21 0.0; 0.009721119932251493 0.0005577606635759174 … -4.103956056185978e-22 0.0; … ; 5.070884373413702e-21 3.187089130763981e-22 … 0.0005577606635759174 0.0; 0.0 0.0 … 0.0 1.5213716556903334e-7;;; 0.34568900956497944 0.009776975998609085 … -6.156801209714805e-21 0.0; 0.009776975998609085 0.0005593606635759175 … -4.104388227819965e-22 0.0; … ; 5.1027683570758455e-21 3.187561317863888e-22 … 0.0005593606635759175 0.0; 0.0 0.0 … 0.0 8.375263600891277e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.5024875623474667 2.1229816298321444e-31 0.0; 0.049751250133412535 1.2032831145192752e-28 0.0; … ; -1.2414004669719228e-28 0.049751250133412535 0.0; 0.0 0.0 0.9999999752491522;;; 0.5098039231064976 -8.354453207068883e-30 0.0; 0.09803924047673965 -3.131300859215219e-28 0.0; … ; -1.0351894275620513e-27 0.09803924047673965 0.0; 0.0 0.0 0.999999975249155;;; … ;;; 0.054705033709726854 -1.678954910837624e-20 0.0; 0.001547195962072148 -9.830832489442495e-22 0.0; … ; 8.069179262857482e-22 0.001547195962072148 0.0; 0.0 0.0 0.9759866455818303;;; 0.05499827263550295 -1.696971285252791e-20 0.0; 0.0015553791891602389 -9.88833768490497e-22 0.0; … ; 8.113414997461925e-22 0.0015553791891602389 0.0; 0.0 0.0 0.9782848225201974;;; 0.05241135673068665 -1.541475971729123e-20 0.0; 0.0014823282274877797 -9.19117477450877e-22 0.0; … ; 7.507072213514851e-22 0.0014823282274877797 0.0; 0.0 0.0 0.8433959372095443])
Código
trayectoria_xy(x, z_true)
Código
trayectoria_t(t, x, z_true)
Código
plot_error(t, x, z_true, P)
Código
plot_gain(t[k1:k2], P[:,:,k1:k2], K[:,:,k1:k2])